#include "gtaxis.h"

GTAxis::GTAxis(QString name, QObject *parent) : SCAxis(name, parent) {}

void GTAxis::setMoveMode(GTAxis::MoveMode mode, bool checkCurrentMode)
{
    if (checkCurrentMode && mode == currentMode)
    {
        return;
    }
    if (mode == Pos)
    {
        short res = GTN_PrfTrap(coreNo, index);
        if (res != CMD_SUCCESS)
        {
            throw ActionError(name(), QString("GTN_PrfTrap failed! Error code: %1").arg(res));
        }
        currentMode = Pos;
        scaleMaxAccImpl(lastAccRatio);
        scaleMaxVelImpl(lastVelRatio);
    }
    else if (mode == Jog)
    {
        short res = GTN_PrfJog(coreNo, index);
        if (res != CMD_SUCCESS)
        {
            throw ActionError(name(), QString("GTN_PrfJog failed! Error code: %1").arg(res));
        }
        currentMode = Jog;
        scaleMaxAccImpl(lastAccRatio);
    }
}

double GTAxis::getCurrentOutputPos()
{
    double value = 0;
    GTN_GetPlsPos(coreNo, index, &value);
    return value / gtAxisConfig->scale();
}

void GTAxis::setFeedbackPos(double pos)
{
    short res = GTN_SetEncPos(coreNo, index, pos * gtAxisConfig->scale());
    printError(res, QString("%1 GTN_SetEncPos failed!").arg(name()));
}

void GTAxis::setCurrentPos(double pos)
{
    short res = GTN_SetPlsPos(coreNo, index, pos * gtAxisConfig->scale());
    printError(res, QString("%1 GTN_SetEncPos failed!").arg(name()));
}

bool GTAxis::hasAlarm()
{
    long status = 0;
    GTN_GetSts(coreNo, index, &status);
    return (status & (1 << 1));
}

bool GTAxis::isInPos() const
{
    long status = 0;
    GTN_GetSts(coreNo, index, &status);
    return (status & (1 << 11));
}

bool GTAxis::isRunning() const
{
    return !isInPos();
}

double GTAxis::getCurrentVel()
{
    double value = 0;
    GTN_GetPlsVel(coreNo, index, &value);
    return value * VelCoeff / gtAxisConfig->scale();
}

void GTAxis::clearStatus()
{
    long status = 0;
    GTN_GetSts(coreNo, index, &status);
    qDebug() << QString("Axis %1 status before clear status: %2").arg(name()).arg(QString::number(status, 16));
    short res = GTN_ClrSts(coreNo, index);
    printError(res, QString("%1 GTN_ClrSts failed!").arg(name()));
    GTN_GetSts(coreNo, index, &status);
    qDebug() << QString("Axis %1 status after clear status: %2").arg(name()).arg(QString::number(status, 16));
}

void GTAxis::initImpl()
{
    gtAxisConfig = qobject_cast<GTAxisConfig *>(config());
    if (gtAxisConfig == nullptr)
    {
        throw SilicolAbort(QString("Can not cast AxisConfig to GTAxisConfig! AxisConfig type: %1")
                               .arg(config()->metaObject()->className()),
                           EX_LOCATION);
    }

    coreNo = gtAxisConfig->coreNo();
    index = gtAxisConfig->index();

    checkResult1(GTN_ClrSts(coreNo, index));
    checkResult1(GTN_PrfTrap(coreNo, index));
    TTrapPrm trapParam;
    checkResult1(GTN_GetTrapPrm(coreNo, index, &trapParam));
    trapParam.acc = gtAxisConfig->maxAcc() * gtAxisConfig->scale() / AccCoeff;
    trapParam.dec = trapParam.acc;
    checkResult1(GTN_SetTrapPrm(coreNo, index, &trapParam));
    checkResult1(GTN_SetAxisBand(coreNo, index, gtAxisConfig->inPosBand() * gtAxisConfig->scale(),
                                 gtAxisConfig->inPosHoldTime()));
    checkResult1(GTN_ZeroPos(coreNo, index));
    currentMode = Pos;
}

void GTAxis::homeImpl()
{
    THomePrm homeParam;
    short res = GTN_GetHomePrm(coreNo, index, &homeParam);
    if (res != CMD_SUCCESS)
    {
        throw ActionError(name(), QString("GTN_GetHomePrm failed! Error code: %1").arg(res));
    }
    homeParam.acc = gtAxisConfig->maxAcc() * gtAxisConfig->scale() / AccCoeff * 0.1;
    homeParam.dec = homeParam.acc;
    homeParam.edge = gtAxisConfig->edge();
    homeParam.mode = GTAxisConfig::GTHomeModeEnumInfo().nameToEnum(gtAxisConfig->gtHomeMode());
    homeParam.velLow = gtAxisConfig->velLow() * gtAxisConfig->scale() / VelCoeff;
    homeParam.moveDir = gtAxisConfig->homeDir();
    homeParam.velHigh = gtAxisConfig->velHigh() * gtAxisConfig->scale() / VelCoeff;
    homeParam.escapeStep = gtAxisConfig->escapeStep() * gtAxisConfig->scale();
    homeParam.indexDir = gtAxisConfig->indexDir();
    homeParam.homeOffset = gtAxisConfig->gtHomeOffset() * gtAxisConfig->scale();
    homeParam.searchHomeDistance = 0;
    homeParam.searchIndexDistance = 0;
    res = GTN_GoHome(coreNo, index, &homeParam);
    if (res != CMD_SUCCESS)
    {
        throw ActionError(name(), QString("GTN_GoHome failed! Error code: %1").arg(res));
    }
}

bool GTAxis::isHomeDone()
{
    GTN_GetHomeStatus(coreNo, index, &homeStatus);
    return (homeStatus.run == 0);
}

QString GTAxis::homeErrorMsg()
{
    GTN_GetHomeStatus(coreNo, index, &homeStatus);
    switch (homeStatus.error)
    {
        case HOME_ERROR_NONE:
            return "";
        case HOME_ERROR_NOT_TRAP_MODE:
            return "HOME_ERROR_NOT_TRAP_MODE";
        case HOME_ERROR_DISABLE:
            return "HOME_ERROR_DISABLE";
        case HOME_ERROR_ALARM:
            return "HOME_ERROR_ALARM";
        case HOME_ERROR_STOP:
            return "HOME_ERROR_STOP";
        case HOME_ERROR_STAGE:
            return "HOME_ERROR_STAGE";
        case HOME_ERROR_HOME_MODE:
            return "HOME_ERROR_HOME_MODE";
        case HOME_ERROR_SET_CAPTURE_HOME:
            return "HOME_ERROR_SET_CAPTURE_HOME";
        case HOME_ERROR_NO_HOME:
            return "HOME_ERROR_NO_HOME";
        case HOME_ERROR_SET_CAPTURE_INDEX:
            return "HOME_ERROR_SET_CAPTURE_INDEX";
        case HOME_ERROR_NO_INDEX:
            return "HOME_ERROR_NO_INDEX";
        default:
            return QString("Unknown error. Code: %1").arg(homeStatus.error);
    }
}

void GTAxis::operationAfterHome()
{
    setCurrentPos(0);
    setFeedbackPos(0);
    short res = GTN_ZeroPos(coreNo, index);
    printError(res, QString("%1 GTN_ZeroPos failed! ").arg(name()));
    resetMaxVel();
    resetMaxAcc();
    GTN_ClrSts(coreNo, index);
}

void GTAxis::stopImpl()
{
    short res = GTN_Stop(coreNo, 1 << (index - 1), 0);
    printError(res, QString("%1 GTN_Stop failed!").arg(name()));
}

void GTAxis::stopHome()
{
    stopImpl();
}

void GTAxis::enableImpl()
{
    short res = GTN_AxisOn(coreNo, index);
    printError(res, QString("%1 GTN_AxisOn failed!").arg(name()));
}

void GTAxis::disableImpl()
{
    short res = GTN_AxisOff(coreNo, index);
    printError(res, QString("%1 GTN_AxisOff failed!").arg(name()));
}

void GTAxis::clearErrorImpl()
{
    short res = GTN_SetDoBit(coreNo, MC_CLEAR, index, 1);
    printError(res, QString("%1 GTN_SetDoBit failed!").arg(name()));
    QThread::msleep(50);
    GTN_SetDoBit(coreNo, MC_CLEAR, index, 0);
    QThread::msleep(10);
    res = GTN_ClrSts(coreNo, index);
    printError(res, QString("%1 GTN_ClrSts failed!").arg(name()));
}

void GTAxis::moveToImpl(double targetPos)
{
    setMoveMode(Pos);
    short res = GTN_SetPos(coreNo, index, targetPos * gtAxisConfig->scale());
    if (res != CMD_SUCCESS)
    {
        throw ActionError(name(), QString("GTN_SetPos failed! Error code: %1").arg(res));
    }
    res = GTN_Update(coreNo, 1 << (index - 1));
    if (res != CMD_SUCCESS)
    {
        throw ActionError(name(), QString("GTN_Update failed! Error code: %1").arg(res));
    }
}

void GTAxis::velocityMoveImpl(SCAxis::Direction dir)
{
    setMoveMode(Jog);
    double vel = gtAxisConfig->maxVel() * gtAxisConfig->scale() / VelCoeff * lastVelRatio;
    if (dir == Direction::Negative)
    {
        vel *= -1;
    }
    short res = GTN_SetVel(coreNo, index, vel);
    printError(res, QString("%1 GTN_SetVel failed!").arg(name()));
    res = GTN_Update(coreNo, 1 << (index - 1));
    printError(res, QString("%1 GTN_Update failed!").arg(name()));
}

double GTAxis::getFeedbackPosImpl()
{
    double value = 0;
    GTN_GetEncPos(coreNo, index, &value);
    return value / gtAxisConfig->scale();
}

void GTAxis::changeTargetPosOnlineImpl(double targetPos)
{
    moveToImpl(targetPos);
}

void GTAxis::scaleMaxAccImpl(double ratio)
{
    lastAccRatio = ratio;
    if (currentMode == Pos)
    {
        TTrapPrm trapParam;
        try
        {
            checkResult1(GTN_GetTrapPrm(coreNo, index, &trapParam));
            trapParam.acc = gtAxisConfig->maxAcc() * gtAxisConfig->scale() / AccCoeff * ratio;
            trapParam.dec = trapParam.acc;
            checkResult1(GTN_SetTrapPrm(coreNo, index, &trapParam));
        }
        catch (SilicoolException &se)
        {
            qCCritical(motionCate()) << se.what();
        }
    }
    else if (currentMode == Jog)
    {
        TJogPrm jogParam;
        try
        {
            checkResult1(GTN_GetJogPrm(coreNo, index, &jogParam));
            jogParam.acc = gtAxisConfig->maxAcc() * gtAxisConfig->scale()  * ratio / AccCoeff;
            jogParam.dec = jogParam.acc;
            checkResult1(GTN_SetJogPrm(coreNo, index, &jogParam));
        }
        catch (SilicoolException &se)
        {
            qCCritical(motionCate()) << se.what();
        }
    }
}

void GTAxis::scaleMaxVelImpl(double ratio)
{
    lastVelRatio = ratio;
    if (currentMode == Pos)
    {
        short res = GTN_SetVel(coreNo, index, gtAxisConfig->maxVel() * gtAxisConfig->scale() * ratio / VelCoeff);
        printError(res, QString("%1 GTN_SetVel failed!").arg(name()))
    }
}
